#ifndef NAVIGATION_HANDLER_H
#define NAVIGATION_HANDLER_H

#include <ros/ros.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <signal.h>
#include <termios.h>
#include <linux/input.h>
#include <navigation/utils/local_planner/path_follower.h>
#include <navigation/utils/local_planner/mover.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <navfn/MakeNavPlan.h>
#include <tecbot_msgs/cmd_vel_mode_request.h>
#include <tecbot_msgs/nav_to_request.h>
#include <tecbot_msgs/nav_to_simple_request.h>
#include <tecbot_msgs/nav_to_state.h>
#include <tecbot_msgs/obstacle.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int8.h>
#include <common/common_state_code.h>
#include <navigation/navigation_state_code.h>
#include <localization/localization_sources.h>

#define NAV_DEBUG 0

#define SERVICE_WAIT_DURATION 30
#define SERVICE_RETRY_DURATION 1

using namespace std;
using namespace navigation_ns;

// 定义状态
enum navigation_mode
{
    navigation_map,
    navigation_odom,
    move_map,
    move_odom,
    none
};

navigation_state state = navigation_state::ready;
navigation_mode mode = navigation_mode::none;

// 定义日志
struct navigation_log
{
    float timestamp;
    float x;
    float y;
    float yaw;
    float path_x;
    float path_y;
    float path_yaw;
    float vx;
    float vyaw;
    float vx_ctrl;
    float vyaw_ctrl;
};

vector<navigation_log> log_content;

// 定义参数
// 通用部分
bool enable_log;
string log_filename;

float control_freq;
float max_x_vel, min_x_vel, max_x_acc;
float max_yaw_vel, min_yaw_vel, max_yaw_acc;
float velocity_dec_line_offset_time, velocity_dec_angle_offset_time;
float strighting_stop_distance, turning_stop_distance, ignore_obstacle_dist;
float obstacle_acc_ratio;
float strighting_tolerance, turning_tolerance;
float sensor_msg_timeout;

// 自主导航部分
bool use_global_planner;
bool allow_reverse_drive;
bool use_advanced_evaluation;
bool use_kinematic_limits;
bool use_find_mode_follower;
float find_mode_x_vel, find_mode_yaw_vel, find_mode_radius, find_mode_min_dist_timeout;
bool enable_near_angle_correct;
float angle_diff_to_forward, angle_diff_to_rotate;
float xy_tolerance, yaw_tolerance;
float chassis_radius;
float sim_time;
int sim_vx_space_size, sim_vyaw_space_size, trajectory_sim_step;
float follow_point_weight, follow_path_weight;
int max_global_plan_try_count;

// 标识部分
bool pose_received = false;
bool odom_received = false;
bool odom_twist_received = false;
bool obstacle_received = false;
bool request_from_simple_goal = false;
bool button_received = false;
bool button_required = false;
int last_light_color = -1;

// 任务开始处理时间
ros::Time cmd_received_time;

// nav_to_state 话题发布器
ros::Publisher nav_to_state_pub;

// 灯光颜色发布器
ros::Publisher light_pub;

// 全局路径规划服务
ros::ServiceClient global_planner_client;

// 接管速度控制服务
ros::ServiceClient cmd_vel_mode_client;

// 定义机器人全局位姿数据
geometry_msgs::PoseWithCovarianceStamped pose_data;
ros::Time pose_data_last_received_time;

// 定义机器人里程计数据
geometry_msgs::PoseWithCovarianceStamped odom_data;
ros::Time odom_data_last_received_time;

// 定义机器人里程计速度数据
geometry_msgs::TwistWithCovarianceStamped odom_twist_data;

// 定义obstacle数据
tecbot_msgs::obstacle obstacle_data;
ros::Time obstacle_data_last_received_time;

// 定义cmd_vel数据
geometry_msgs::Twist twist_data;

// 定义button数据
std_msgs::Bool button_data;

// 定义 light 数据
std_msgs::Int8 light_data;

// 定义目标点数据
tecbot_msgs::nav_to_request::Request nav_to_request_data;

// 定义导航状态数据
tecbot_msgs::nav_to_state nav_to_state_data;
block_state obstacle_state_data;

// 定义移动控制器
unique_ptr<mover> mover_controller;

// 定义自主导航控制器
unique_ptr<path_follower> path_follower_controller;

#endif